#ifndef __MATHS_H
#define __MATHS_H

#include <vector>

#include "concrete/Vector3D.h"

typedef float Scalar;
typedef Vector3D Vector;

// Note: This is a 2D orientation!
class Orientation
{
	Scalar m_angle;
public:
   
	// TODO: Add getters and setters as needed
    inline const Scalar& getAngle() const { return m_angle; }
    //Vector getVector();
    //Matrix getMatrix();
};

class Xform
{
	Vector      m_pos;
	Orientation m_ori;
public:
	Xform() {}
	Xform(const Vector& pos, const Orientation& ori) : m_pos(pos), m_ori(ori) {}
	
	inline const Vector& getPos() const { return m_pos; }
	inline Vector&       getPos()       { return m_pos; }
	inline void          setPos(const Vector& pos)
	{ m_pos = pos; }
	
	inline const Orientation& getOri() const { return m_ori; }
	inline Orientation&       getOri()       { return m_ori; }
	inline void               setOri(const Orientation& ori)
	{ m_ori = ori; }
	// ... + operators
	
};

class AABB
{
	Vector m_start, m_end;
public:
	AABB() {}
	AABB(const Vector& start, const Vector& end) : m_start(start), m_end(end) {}
	
	inline const Vector& getStart() const { return m_start; }
	inline Vector& getStart() { return m_start; }
	inline const Vector& getEnd() const { return m_end; }
	inline Vector& getEnd() { return m_end; }
	
	inline bool overlaps(const AABB& other)
	{
		// TODO: Add comparison operators to Vector (component-wise)
		return (this->getStart() <= other.getEnd() &&
				this->getEnd() >= other.getStart());
	}
};

// Oriented bounding box
class OBB
{
    AABB        m_bb;
    Orientation m_orientation;
public:
    OBB() {}
    OBB(const AABB& aabb, const Orientation& orientation)
        : m_bb(aabb), m_orientation(orientation) {}
   
    inline const AABB& getBB() const                 { return m_bb; }
    inline AABB& getBB()                             { return m_bb; }
    inline const Orientation& getOrientation() const { return m_orientation; }
    inline Orientation& getOrientation()             { return m_orientation; }
   
    bool intersects(const OBB& other);
};

// Bounding sphere/circle
class Circle
{
    Vector m_pos;
    Scalar m_radius;
public:
    Circle() {}
    Circle(const Vector& pos, const Scalar& radius)
        : m_pos(pos), m_radius(radius) {}
   
    inline const Vector& getPos() const    { return m_pos; }
    inline Vector& getPos()                { return m_pos; }
    inline const Scalar& getRadius() const { return m_radius; }
    inline Scalar& getRadius()             { return m_radius; }
   
    bool intersects(const Circle& other)
        {
            Scalar r = m_radius + other.getRadius();
            Scalar dist2 = (m_pos - other.getPos()).getLength2();
            return (dist2 < r*r);
        }
};



#if 0

class Shape
{
public:
    virtual int collide(const Shape* other, Collision& collision) const { return other->collide(this, collision); }
};

class CircleShape
{
    Vector m_pos;
    Scalar m_radius;
public:
    CircleShape(Vector pos, Scalar radius) : m_pos(pos), m_radius(radius) {}
    
    inline Vector getPos() const    { return m_pos; }
    inline Scalar getRadius() const { return m_radius; }
    
    //virtual int collide(const Shape* other, Collision& collision) const { return other->collide(this, collision); }
    int collide(const CircleShape* other, Collision& collision) const
    {
        Vector between = other->getPos() - m_pos;
        // Check for intersection
        Scalar dist2 = between.getLength2();
        Scalar tot_radius = m_radius + other->getRadius();
        if( dist2 < tot_radius*tot_radius ) {
            Scalar dist = sqrt(dist2);
            
            collision.normal = between / dist;
            collision.penetration = tot_radius - dist;
            collision.pos = m_pos + collision.normal * (m_radius - 0.5*collision.penetration);
            collision.lambda = 0.5;
            //collision.object1 = this;
            //collision.object2 = other;
            
            return 1;
        }
        else {
            return 0;
        }
    }
};

class SegmentShape
{
    Vector m_start;
    Vector m_end;
    Scalar m_width;
public:
    SegmentShape(Vector start, Vector end, Scalar width)
     : m_start(start), m_end(end), m_width(width) {}
    
    inline Vector getStart() const   { return m_start; }
    inline Vector getEnd() const     { return m_start; }
    inline Vector getTangent() const { return m_end - m_start; }
    inline Vector getNormal() const  { return this->getTangent().getNormal(); }
    
    //virtual int collide(const Shape* other, Collision& collision) const { return other->collide(this, collision); }
    int collide(const CircleShape* circle, Collision& collision) const
    {
        Vector between = circle->getPos() - m_start;
        Scalar lambda = (between % this->getTangent()) / this->getTangent().getLength2();
        // If circle centre is between the ends of the segment
        if( lambda <= 0.0 ) {
            // Collision occurs with segment start, which we already have
        }
        else if( lambda >= 1.0 ) {
            // Collision occurs with segment end
            between = circle->getPos() - m_end;
        }
        else {
            // Project between onto sement normal
            between = between % this->getNormal() * this->getNormal().asUnitVector();
        }
        
        // Check for intersection just like with two circles
        Scalar dist2 = between.getLength2();
        Scalar tot_radius = m_width + circle->getRadius();
        if( dist2 < tot_radius * tot_radius ) {
            Scalar dist = sqrt(dist2);
            
            collision.normal = between / dist;
            collision.penetration = tot_radius - dist;
            collision.pos = circle->getPos() -
                            collision.normal * (circle->getRadius() - 0.5*collision.penetration);
            collision.lambda = lambda;
            //collision.object1 = this;
            //collision.object2 = circle;
            
            return 1;
        }
        else {
            return 0;
        }
    }
};

class PolygonShape
{
    std::vector<Vector3D> m_points;
public:
	inline Vector3D addPoint(Vector3D p)      { m_points.push_back(p); return p; }
	inline Vector3D getPoint(int index) const { return m_points[index]; }
	inline int getPointCount() const 	      { return m_points.size(); }
	
	//virtual int collide(const Shape* other, Collision& collision) const { return other->collide(this, collision); }
	int collide(const CircleShape* circle, Collision& collision) const
	{
    	int n = this->getPointCount();
    	//vector<Vector> betweenVectors(n);
    	//vector<Vector> edgeVectors(n);
    	Vector closest_between;
    	Vector closest_edge;
    	
    	Scalar min_dist = 1e99; 	//Initialise the minimum distance between the point and the edge to infinity
    	int i_closest = 0; 			//Initialise the index of the closest edge
    	
    	//Loop through all points in the polygon
    	for( int i=0; i<n; i++ ) {
    		
    		CircleShape point(m_points[i], 0.0);
    		// Check for circle vs point collision
    		if( circle->collide(&point, collision) )
                return 1;
            
    		//Calculate edge vector
    		int j = i + 1;
    		if( j >= n ) j = 0;
    		Vector edge = m_points[j] - m_points[i];
    		Vector normal = edge.getNormal().asUnitVector();
    		Vector test_pos = circle->getPos() - circle->getRadius() * normal;
    		Vector between = test_pos - m_points[i];
    		
    		//Make sure the point is inside the polygon
    		if( between * edge > 0.0 ) { // 2D cross product
    			//Circle can't have collided
    			return 0;
    		}
    		
    		//Find the closest edge
    		// TODO: Check this
    		Scalar dist = fabs((between * edge) / edge.getLength2());
    		if( dist < min_dist ) {
                min_dist = dist;
                i_closest = i;
                closest_between = between;
                closest_edge = edge;
            }
    	}
    	
    	//Calculate the position along the edge that the collision occurred
    	Scalar lambda = (closest_between % closest_edge) / closest_edge.getLength2();
    	
    	if( lambda < 0.0 || lambda > 1.0 ) {
    		return 0;
    	}
    	
    	//Set collision data
    	collision.lambda = lambda;
    	collision.pos = m_points[i_closest] + lambda * closest_edge;
    	collision.normal = closest_edge.getNormal().asUnitVector();
    	collision.penetration = min_dist;
    }
	
};

#endif //0

#endif // __MATHS_H
